//
// Created by LiuDongPeng on 2024/1/29.
//

#ifndef FOC_MOTOR_H
#define FOC_MOTOR_H


#include "Stm32Base.h"
#include "EncoderBase.h"


class Motor
{
public:
    Motor();
    ~Motor() = delete;

    enum class MotorStatus
    {
        MOTOR_STATUS_IDLE,
        MOTOR_STATUS_INIT,
        MOTOR_STATUS_START,
        MOTOR_STATUS_RUN,
        MOTOR_STATUS_STOP,
        MOTOR_STATUS_BRAKE,
        MOTOR_STATUS_WAIT,
        MOTOR_STATUS_FAULT,
    };


    bool link_encoder(EncoderBase* encoder);
    bool has_encoder() const;
    EncoderBase* get_encoder() const;
    void align_encoder(float ud, float angle);
    std::tuple<float, float> calc_sin_cos_theta();
    std::tuple<float, float> sin_cos_theta(float theta);
    float getAngleRad() const;
    void setAngleRad(float angleRad);
    float getAngleRadOffset() const;
    void setAngleRadOffset(float angleRadOffset);
    float getTheta();
    void setTheta(float theta);

    void set_ia_ib_ic(float ia, float ib, float ic);

    void open_loop_test(float ud, float uq);
    void idq_pi_ctrl(float idRef, float iqRef);
    void current_closed_loop(float idRef, float iqRef);
    void speed_closed_loop(float speedRef);
    void position_closed_loop(float posRef);
    void apply_pwm_duty(float ta, float tb, float tc) const;

    TIM_HandleTypeDef *getTim() const;
    void setTim(TIM_HandleTypeDef *htim);


    MotorStatus get_status() const;
    void set_status(MotorStatus status);

    void enable();
    void disable();


public:

    TIM_HandleTypeDef* tim_;

    uint32_t timArr_ = 4200;

    EncoderBase* encoder_;

    /* Motor param */
    int polePairs_ = 7;
    float Rs_, Ld_;

    float VBus_ = 12.0f, IBus_;
    float ia_, ib_, ic_;
    float ialpha_, ibeta_;
    float id_, iq_;
    float ud_, uq_;
    float ualpha_, ubeta_;

    float ta_, tb_, tc_;

    float angleRad_, angleRadOffset_, theta_;
    float sinTheta_, cosTheta_;

    MotorStatus status_ = MotorStatus::MOTOR_STATUS_STOP;
};


#endif //FOC_MOTOR_H
